
#include "CommonBehaviours.h"
#include "../Utils/Vector2D.h"
#include "../Ship.h"
#include "../World.h"
#include "../Map.h"

#include <cassert>
#include <cmath>


bool CommonBehaviours::canSeePoint(Ship *ship, Vector2D point, bool use_collision_space, 
                                   bool tight_collision_space){
    assert(ship != NULL);

    Vector2D tmp1, tmp2;
    return !World::instance().getMap()->collideRaySegment(ship->getPosition(), 
                point, tmp1, tmp2, use_collision_space, tight_collision_space);
}


bool CommonBehaviours::goDirectToPoint(Ship *ship, Vector2D point){
    assert(ship != NULL);

    Vector2D to_point = point - ship->getPosition();
    if(to_point.length() < ship->getSize()){ return true; }
    to_point.normalise();
    
    Vector2D velocity = ship->getVelocity();
    velocity.normalise();
    
    float v_angle = acosf(to_point.dotProduct(velocity)) * 180.0f/M_PI;
    if(v_angle != v_angle){ // We need to check for "nan" here because acos(1.0f) = nan, but should be 0 
        v_angle = 0.0f;
    }

    float h_angle = acosf(to_point.dotProduct(ship->getHeading())) * 180.0f/M_PI;
    if(h_angle != h_angle){ // We need to check for "nan" here because acos(1.0f) = nan, but should be 0 
        h_angle = 0.0f;
    }

    // Our current velocity is a fairly close match or very much not a match with 
    // the desired velocity, therefore just point the ship in the desired direction.
    if(ship->getVelocity().length() < 0.1f){
        to_point.rotate(-ship->getRotation());
        if(to_point.x < 0.0f){ ship->turn(h_angle); }
        else { ship->turn(-h_angle); }

        if(h_angle < 80.0f){
            ship->accelerate(1.0f, false);
        }
    } 
    else{
        float v_rotation = acosf(velocity.dotProduct(Vector2D(0.0f, 1.0f))) * 180.0f/M_PI;
        if(velocity.x > 0.0f){ v_rotation = -v_rotation; }

        Vector2D to_point2 = to_point;
        to_point2.rotate(-v_rotation);

        float turn = 0.0f;
        if(to_point2.x > 0.0f){ 
            float desired_velocity_r = v_rotation - 2.0f*v_angle;
            
            turn = desired_velocity_r - ship->getRotation();
            if(fabs(turn) > 80.0f){
                to_point.rotate(-ship->getRotation());
                if(to_point.x < 0.0f){ ship->turn(h_angle); }
                else { ship->turn(-h_angle); }
                
                if(h_angle < 80.0f){ ship->accelerate(1.0f, false); }
            }
            else{
                ship->turn(desired_velocity_r - ship->getRotation());
                ship->accelerate(1.0f, false);
            }
        }
        else{
            float desired_velocity_r = v_rotation + 2.0f*v_angle;
            turn = desired_velocity_r - ship->getRotation();
            
            if(fabs(turn) > 80.0f){
                to_point.rotate(-ship->getRotation());
                if(to_point.x < 0.0f){ ship->turn(h_angle); }
                else { ship->turn(-h_angle); }
                
                if(h_angle < 80.0f){ ship->accelerate(1.0f, false); }
            }
            else{
                ship->turn(desired_velocity_r - ship->getRotation());
                ship->accelerate(1.0f, false);
            }
        }
//        ship->accelerate(1.0f);
    }
   
    return false;    
}

